#!/usr/bin/env python3


import rclpy
from rclpy.node import Node
from arduinobot_msgs.srv import AddTwoints
from arduinobot_msgs.srv import EulerToQuaternion
from arduinobot_msgs.srv import QuaternionToEuler
from tf_transformations import quaternion_from_euler
from tf_transformations import euler_from_quaternion


class AnglesConverter(Node):
    def __init__(self):
        super().__init__('angle_conversion_service_server')
        self.euler_to_quaternion = self.create_service(EulerToQuaternion, 'euler_to_quaternion', self.eulerToQuaternionCallback)
        self.quaternion_to_euler = self.create_service(QuaternionToEuler, 'quaternion_to_euler', self.quaternionToEulerCallback)
        self.get_logger().info('Service server is ready to receive requests on "euler_to_quaternion" and "quaternion_to_euler" topics')
        
    def eulerToQuaternionCallback(self, request, response):
        self.get_logger().info('Requested to convert euler angle roll:%f pitch:%f yaw:%f' % (request.roll , request.pitch , request.yaw))
        (response.x, response.y, response.z, response.w) = quaternion_from_euler(request.roll, request.pitch, request.yaw)
        self.get_logger().info('Converted to quaternion x:%f y:%f z:%f w:%f' % (response.x , response.y , response.z , response.w))
        return response
    

    def quaternionToEulerCallback(self, request, response):
        self.get_logger().info('Requested to convert quaternion x:%f y:%f z:%f w:%f' % (request.x , request.y , request.z , request.w))
        (response.roll, response.pitch, response.yaw) = euler_from_quaternion(request.x, request.y, request.z, request.w)
        self.get_logger().info('Converted to euler angle roll:%f pitch:%f yaw:%f' % (response.roll , response.pitch , response.yaw))
        return response
    


def main():
    rclpy.init()
    angles_converter = AnglesConverter()
    rclpy.spin(angles_converter)
    angles_converter.destroy_node()
    rclpy.shutdown()
if __name__ == '__main__':
    main()
# This code defines a simple ROS 2 service server that listens for requests on the 'chatter' topic.
